class_name SteeringBehaviorAgent
extends Node2D

## 参考：
## https://natureofcode.com/autonomous-agents/
## https://gdquest.gitbook.io/godot-3-steering-ai-framework-reference
## https://github.com/GDQuest/godot-steering-ai-framework

#region 属性
@export_group("属性")
## 当速度小于这个值时认为已经停止运动
@export var zero_lineaar_speed_threshold_cp:CompositeProperty
var zero_lineaar_speed_threshold:float:
	get:
		return zero_lineaar_speed_threshold_cp.get_value(entity)
## 最大线速度
@export var linear_speed_max_cp:CompositeProperty
var linear_speed_max:float:
	get:
		return linear_speed_max_cp.get_value(entity)
## 最大线性加速度
@export var linear_acceleration_max_cp:CompositeProperty
var linear_acceleration_max:float:
	get:
		return linear_acceleration_max_cp.get_value(entity)
## 最大角速度
@export var angular_speed_max_cp:CompositeProperty
var angular_speed_max:float:
	get:
		return angular_speed_max_cp.get_value(entity)
## 最大角加速度
@export var angular_acceleration_max_cp:CompositeProperty
var angular_acceleration_max:float:
	get:
		return angular_acceleration_max_cp.get_value(entity)
## 线性摩擦力
@export var linear_friction_cp:CompositeProperty
var linear_friction:float:
	get:
		return linear_friction_cp.get_value(entity)
## 角摩擦力
@export var angular_friction_cp:CompositeProperty
var angular_friction:float:
	get:
		return angular_friction_cp.get_value(entity)
## 代理半径
@export var body_radius_cp:CompositeProperty
var body_radius:float:
	get:
		return body_radius_cp.get_value(entity)
## 代理质量
@export var body_mass_cp:CompositeProperty
var body_mass:float:
	get:
		return body_mass_cp.get_value(entity)
## 是否激活
@export var enable:bool = true
@export_group("")
@export_group("引用")
@export var entity:Entity
@export_group("")
## 当前线速度
var linear_velocity:Vector2
## 当前线性加速度
var linear_acceleration:Vector2
## 当前角速度
var angular_velocity:float
## 当前角加速度
var angular_acceleration:float
## 由群体行为和邻近性在内部使用，将代理标记为已考虑
var is_tagged:bool = false
## 当前倾向
var linear_force_interest:Vector2 = Vector2.ZERO
var angular_force_interest:float
## 当前行为
var behaviors_owner:Node
var behaviors:Array[BaseContextBehavior] = []
var connect_obj_dict:Dictionary = {}

#endregion
#region 字段
#endregion
#region 信号
#endregion

#region 基类方法
func init():
	pass
#endregion
#region 公共方法
func get_pos():
	return global_position
func get_entity():
	return entity
func get_entity_root():
	return entity.root
## 获得碰撞的layer_mask
func get_obstacle_layer_mask() -> int:
	return 0
## 获得当前阵营
func get_camp() -> GameEnum.Camp:
	return entity.entity_data.runtime_camp
## 获取当前线速度
func get_vel() -> Vector2:
	return linear_velocity
## 获得当前角速度
func get_angular() -> float:
	return angular_velocity
## 获得当前Rotation
func get_rot() -> float:
	return entity.root.rotation
## 添加线速度力倾向
func add_linear_force_interest(interest:Vector2):
	linear_force_interest += interest
## 添加角速度倾向
func add_angular_forcce_interest(interest:float):
	angular_force_interest += interest
## 根据现在的interest更新转向结果
func update_steering(delta:float):
	# 线性部分
	var dir = linear_force_interest.normalized()
	var acc = linear_force_interest / body_mass
	linear_acceleration = min(acc.length(), linear_acceleration_max) * dir
	
	# 应用线性摩擦力
	if linear_velocity.length() > zero_lineaar_speed_threshold:
		# 计算摩擦方向（与当前速度相反）
		var friction_dir = -linear_velocity.normalized()
		# 应用摩擦减速，但保持最小速度
		var friction_force = friction_dir * linear_friction * delta
		linear_velocity += friction_force
		# 如果摩擦力导致速度反向，则停止
		if linear_velocity.dot(friction_dir) > 0:
			linear_velocity = Vector2.ZERO
	
	# 应用加速度
	linear_velocity = linear_velocity + linear_acceleration * delta
	if linear_velocity.length() > linear_speed_max:
		linear_velocity = linear_velocity.normalized() * linear_speed_max
	
	# 角速度部分（重写）
	# 1. 计算目标角速度
	var target_angular = angular_force_interest / body_mass
	target_angular = clamp(target_angular, -angular_acceleration_max, angular_acceleration_max)
	
	# 2. 应用角速度摩擦
	if abs(angular_velocity) > zero_lineaar_speed_threshold:
		# 计算摩擦方向（与当前角速度相反）
		var friction_dir = -sign(angular_velocity)
		# 应用摩擦减速
		angular_velocity += friction_dir * angular_friction * delta
		# 确保不会反向加速
		if sign(angular_velocity) != sign(angular_velocity - friction_dir * angular_friction * delta):
			angular_velocity = 0
	
	# 3. 应用目标角速度
	angular_velocity = move_toward(angular_velocity, target_angular, angular_acceleration_max * delta)
	
	# 4. 限制最大角速度
	if abs(angular_velocity) > angular_speed_max:
		angular_velocity = sign(angular_velocity) * angular_speed_max
## 重置interest
func reset_interest():
	linear_force_interest = Vector2.ZERO
	angular_force_interest = 0
func set_enable(enable):
	if self.enable == enable:
		return
	self.enable = enable
## 执行行为
# behaviors_owner：记录behaviors的对象
# behaviors：行为数组
func excute_behaviors(delta: float):
	if not enable:
		return
	reset_interest()
	for behavior in behaviors:
		if not behavior.enable:
			continue
		behavior.calc_steering(behaviors_owner,self,delta)
		GameSteeringBehaviorDebug.add_draw_listener(func():
			if behavior.is_debug:
				behavior.draw()
		)
	update_steering(delta)
	entity.change_vel(get_vel())
	entity.add_rotation(get_angular())
	GameSteeringBehaviorDebug.add_draw_listener(func():
		var start_point = global_position
		var end_point = global_position + get_vel().normalized() * 10
		var v0 = (start_point - end_point).rotated(PI/30) + end_point
		var v1 = (start_point - end_point).rotated(-PI/30) + end_point
		GameSteeringBehaviorDebug.draw_colored_polygon(PackedVector2Array([start_point,v0,end_point,end_point,v1,start_point]),Color.CORNFLOWER_BLUE)
	)
	GameSteeringBehaviorDebug.queue_redraw()
## 更新执行行为列表
func update_behaviors(behaviors_owner:Node,behaviors:Array[BaseContextBehavior]):
	self.behaviors_owner = behaviors_owner
	self.behaviors.clear()
	self.behaviors.append_array(behaviors)
	if not self.behaviors_owner and not self.behaviors:
		reset_interest()
		behaviors.clear()
		behaviors.append_array(behaviors)
		entity.change_vel(get_vel())
		entity.add_rotation(get_angular())
		GameSteeringBehaviorDebug.queue_redraw()
#endregion
#region 私有方法
#endregion
#region 生命周期函数
func _ready() -> void:
	pass
func _physics_process(delta: float) -> void:
	excute_behaviors(delta)
func _notification(what: int) -> void:
	if what == NOTIFICATION_PREDELETE:
		for connect_obj:ConnectObj in connect_obj_dict.values():
			if connect_obj:
				connect_obj.disconnect_all()
#endregion
